#include "matrix.h"

Matrix::Matrix( void ) {
    for( unsigned int i = 0; i < 16; i++ )
        data[i] = 0.0;
}

Matrix::Matrix( float * f ) {
    for( unsigned int i = 0; i < 16; i++ )
        data[i] = f[i];
}

float & Matrix::operator[]( unsigned int i) {
    return data[i];
}

float Matrix::at( unsigned int i ) const {
    return data[i];
}

float Matrix::at( unsigned int i, unsigned int j ) const {
    return data[ i * 4 + j ];
}

void Matrix::set( unsigned int i, unsigned int j, float f ) {
    data[ i *4 + j ] = f;
}

Matrix Matrix::operator*( const Matrix & mat ) const {
    Matrix result;

    for( unsigned int i = 0; i < 4; i++) {
		for( unsigned int j = 0; j < 4; j++) {
			for( unsigned int k = 0; k < 4; k++) {
				result.data[ i * 4 + j ] += data[i * 4 + k] * mat.data[ k * 4 + j ];
			}
	    }
	}

    return result;
}

Vector Matrix::operator*( const Vector & vect ) const {
    Vector result;

    for( unsigned int i = 0; i < 4; i++) {
		for( unsigned int j = 0; j < 4; j++ ) {
		    result[i] += data[i * 4 + j] * vect.at(j);
		}
	}

    return result;
}

Matrix & Matrix::operator*=( const Matrix & mat ) {
    *this = *this * mat;
    return *this;
}

bool Matrix::operator==( const Matrix & mat ) const {
    for( unsigned int i = 0; i < 16; i++ ) {
        if ( data[i] != mat.data[i] )
            return false;
    }

    return true;
}

bool Matrix::equals( const Matrix & mat, float threshold ) const {
    for( unsigned int i = 0; i < 16; i++ ) {
        if ( data[i] <= mat.data[i] - threshold || data[i] >= mat.data[i] + threshold)
            return false;
    }

    return true;
}

std::ostream & operator<<( std::ostream & out, const Matrix & mat ) {
    out << "{";

    for( unsigned int i = 0; i < 16; i++ ) {
        out << " " << mat.data[i];

        if ( i > 15 )
            out << ",";
    }

    out << " }";

    return out;
}
